import java.util.Random;

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.DifferentialPilot;


public class Doomba
{
	private final byte STOP_STATE = 0;
	private final byte FORWARD_STATE = 1;
	private final byte BACKWARD_STATE = -1;
	
	private byte currentState = 0;
	private int left = 0, right = 0;
	private DifferentialPilot pilot;
	private TouchSensor sensorOne = new TouchSensor(SensorPort.S1);
	private TouchSensor sensorTwo = new TouchSensor(SensorPort.S2);
	private TouchSensor sensorThree = new TouchSensor(SensorPort.S3);
	private int wallDistance = 35;
	private int shortWallDistance = 17;
	private Random rand = new Random();
	private boolean recheck = false;
	
	public Doomba()
	{
		pilot = new DifferentialPilot(7, 15, Motor.A, Motor.B);
//		pilot.setTravelSpeed(35);
	}
	
	/**
	 * Busy bee initializer
	 */
	public void init()
	{
		while(true)
		{
			Sonar.printDistance((byte)0, (byte)2);
			
			if(sensorOne.isPressed() || sensorTwo.isPressed() || sensorThree.isPressed() || recheck)
			{
				
				left = right = 0;
				pilot.travel(-5);
				Sonar.lookLeft();
				left = Sonar.getDistance();
				Sonar.lookRight();
				right = Sonar.getDistance();
				recheck = false;

				if (left < shortWallDistance && right < shortWallDistance)
				{
				    recheck = true;
				}
				else if(left < wallDistance && right < wallDistance)
				{
					pilot.rotate(180);
				}
				else if(left > wallDistance && right < wallDistance)
				{
					pilot.rotate(rand.nextFloat() * 50 + 50);
					
				}
				else
				{
					pilot.rotate((rand.nextFloat() * 50 + 50) * -1);
					
				}
				
				currentState = STOP_STATE;
				Sonar.lookAhead();
			}
			
			if(currentState == STOP_STATE)
			{
				pilot.forward();
				currentState = FORWARD_STATE;
			}
			
		}
	}
	
}
